#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>


template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_up0(position ak, index j, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
	 
  if(less(*(ak + j), x)) {
    *(ak + (j >> 1)) = *(ak + 1);
    if(less(*(ak + 1), x)) {
      *(ak + 1) = *(ak); 
      if(less(*(ak), x))	
	*(ak) = x;
      else
	*(ak + 1) = x;	
    }
    else
      *(ak + (j >> 1)) = x;
  }
  else 
    *(ak +  j) = x;
}





template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_up(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  position ak;
  while(1) {
    j = (i & 7) >> 1;
    ak = a + (i & -8);
    if(less(*(ak + j), x)) {
      *(ak + j) = *(ak + 1); 
    }
    else {
      *(a + i) = x;
      return;
    }
    if(less(*(ak + 1), x)) {
      *(ak + 1) = *(ak);
    }
    else {
      *(ak + j) = x;
      return;
    }
    if(less(x, *(ak))) {
      *(ak + 1) = x;
      return;
    }
    else {
      if((i & -8)  != 0) {
	j = (i >> 3) - 1; 
	index t = j & -8;
	j = ((j & 7) >> 1) + 4;
	i = j + t;
	index m = (j >> 1) + t;
	*(ak) = *(a + i);
	if(less(x, *(a + i))) {
	  *(ak) = x;
	  return;
	}
	*(a + i) = *(a + m);
      }
      else {
	*(ak) = x;
	return;
      } 
    }
  }
}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down0(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j;
  
  if((i & 7) && (p.last_leaf() >= (i | 7))) {
    index l = ((((p.last_leaf() >> 3) - 1) >> 3) + 1) << 3;
    if(i >= l && i & 4)
      return;
    index k = (i | 4) & -4;
    while (i < k) {
      j = p.top_all_present(a, i, less);
      if(less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
    if(i > l) {
      *(a + i) = x;
      return;
    }
    else {
      if(p.last_child(i) <= p.last_leaf())
	j = p.top_all_present(a, i, less);
      else {
	if(p.first_child(i) <= p.last_leaf())
	  j = p.top_some_absent(a, i, less);
	else {
	  *(a + i) = x;    
	  return;
	}
      }
      if (less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
  }
  index m = (p.last_leaf() >> 6) << 3;
  while (i < m) {
    const position ak = a + i;
    *(ak) = *(ak + 1);
    if(less(*(ak + 2), *(ak + 3))) {
      *(ak + 1) = *(ak + 3);
      if(less(*(ak + 6), *(ak + 7))) {
	i = ((i | 7) << 4) - (i << 3) - 48;
  	*(ak + 3) = *(ak + 7);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {
	  sift_up0<d, position, index, element, ordering, policy>(ak, 7, x, less, p);      
	  return;
	}
	else
	  *(ak + 7) =  *(a + i); 
      }
      else {
	i = ((i | 6) << 4) - (i << 3) - 48;
	*(ak + 3) = *(ak + 6);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {      
	  sift_up0<d, position, index, element, ordering, policy>(ak, 6, x, less, p);         
	  return;
	}
	else
	  *(ak + 6) =  *(a + i);
      }
    }
    else {
      *(ak + 1) = *(ak + 2);
      if(less(*(ak + 4), *(ak + 5))) {
	i = ((i | 5) << 4) - (i << 3) - 48;
  	*(ak + 2) = *(ak + 5); 
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {
	  sift_up0<d, position, index, element, ordering, policy>(ak, 5, x, less, p);               
	  return;
	}
	else
	  *(ak + 5) =  *(a + i);
      }
      else {
	i = ((i | 4) << 4) - (i << 3) - 48;
	*(ak + 2) = *(ak + 4);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {
	  sift_up0<d, position, index, element, ordering, policy>(ak, 4, x, less, p);              
	  return;
	}
	else
	  *(ak + 4) =  *(a + i);
      }
    }      
  }
  if((((p.last_leaf() >> 3) - 1) & -8) == i) {
    index k = (i | 4) & -4;
    while (i < k) {
      j = p.top_all_present(a, i, less);
      if(less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);
    else {
      if(p.first_child(i) <= p.last_leaf())
	j = p.top_some_absent(a, i, less);
      else {
	*(a + i) = x;    
	return;
      }
    }
    if (less(x, *(a + j))) 
      *(a + i) = *(a + j);    
    else {
      *(a + i) = x;
      return;
    }
    i = j;
  }

  if ((i | 7) <= p.last_leaf()) { 
    const position ak = a + i;
    *(ak) = *(ak + 1);
    if(less(*(ak + 2), *(ak + 3))) {
      *(ak + 1) = *(ak + 3);
      if(less(*(ak + 6), *(ak + 7))) {
  	*(ak + 3) = *(ak + 7);
	sift_up0<d, position, index, element, ordering, policy>(ak, 7, x, less, p); 
      }
      else {
	*(ak + 3) = *(ak + 6);
	sift_up0<d, position, index, element, ordering, policy>(ak, 6, x, less, p);      
      }
    }
    else {
      *(ak + 1) = *(ak + 2);
      if(less(*(ak + 4), *(ak + 5))) {
	*(ak + 2) = *(ak + 5); 
	sift_up0<d, position, index, element, ordering, policy>(ak, 5, x, less, p); 	
      }
      else {
	*(ak + 2) = *(ak + 4);
	sift_up0<d, position, index, element, ordering, policy>(ak, 4, x, less, p);       
      }
    }
    return;
  }


  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      *(a + i) = *(a + j);
    }
    else {
      *(a + i) = x;
      return;
    }
    i = j;
  }
  if(p.first_child(i) <= p.last_leaf()) {
    j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      *(a + i) = *(a + j);
      i = j;
    }
    else {
      *(a + i) = x;
      return;
    }
  }    
  *(a + i) = x;
  return;      

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down1(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());




  index j;


  index m = (p.last_leaf() >> 6) << 3;
  while (i < m) {
    const position ak = a + i;
    *(ak) = *(ak + 1);
    if(less(*(ak + 2), *(ak + 3))) {
      *(ak + 1) = *(ak + 3);
      if(less(*(ak + 6), *(ak + 7))) {
	i = ((i | 7) << 4) - (i << 3) - 48;
  	*(ak + 3) = *(ak + 7);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	*(ak + 7) =  *(a + i); 
      }
      else {
	i = ((i | 6) << 4) - (i << 3) - 48;
	*(ak + 3) = *(ak + 6);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;       
	*(ak + 6) =  *(a + i);
      }
    }
    else {
      *(ak + 1) = *(ak + 2);
      if(less(*(ak + 4), *(ak + 5))) {
	i = ((i | 5) << 4) - (i << 3) - 48;
  	*(ak + 2) = *(ak + 5); 
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	*(ak + 5) =  *(a + i);
      }
      else {
	i = ((i | 4) << 4) - (i << 3) - 48;
	*(ak + 2) = *(ak + 4);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;

	*(ak + 4) =  *(a + i);
      }
    }      
  }
  if(((((p.last_leaf() >> 3) - 1) >> 3) << 3) == i) {

    index k = (i | 4);
    while (i < k) {
      j = p.top_all_present(a, i,  less);
      *(a + i) = *(a + j);    
      i = j;
    }
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i,  less);
    else {
      if(p.first_child(i) <= p.last_leaf())
	j = p.top_some_absent(a, i, less);
      else {
	sift_up<d, position, index, element, ordering, policy>(a, i, x, less, p);
	return;
      }
    }
    *(a + i) = *(a + j);    
    i = j;
  }
  if ((i | 7) <= p.last_leaf()) { 
    const position ak = a + i;
    *(ak) = *(ak + 1);
    if(less(*(ak + 2), *(ak + 3))) {
      *(ak + 1) = *(ak + 3);
      if(less(*(ak + 6), *(ak + 7))) {
  	*(ak + 3) = *(ak + 7);
	sift_up<d, position, index, element, ordering, policy>(a, i + 7, x, less, p);
	return;
      }
      else {
	*(ak + 3) = *(ak + 6);
	sift_up<d, position, index, element, ordering, policy>(a, i + 6, x, less, p);
	return;
      }
    }
    else {
      *(ak + 1) = *(ak + 2);
      if(less(*(ak + 4), *(ak + 5))) {
	*(ak + 2) = *(ak + 5); 
	sift_up<d, position, index, element, ordering, policy>(a, i + 5, x, less, p);
	return;
      }
      else {
	*(ak + 2) = *(ak + 4);
	sift_up<d, position, index, element, ordering, policy>(a, i + 4, x, less, p);
	return;
      }
    }
    return;
  }

  if(less(*(a + i), x) && (i)) {
    j = (i >> 3) - 1; 
    index t = j & -8;
    j = ((j & 7) >> 1) + 4;
    i = j + t;
    m = (j >> 1) + t;
    *(a + i) = *(a + m);
    sift_up<d, position, index, element, ordering, policy>(a, i, x, less, p);
    return;
  }

  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      *(a + i) = *(a + j);
    }
    else {
      *(a + i) = x;
      return;
    }
    i = j;
  }
  if(p.first_child(i) <= p.last_leaf()) {
    j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      *(a + i) = *(a + j);
      i = j;
    }
    else {
      *(a + i) = x;
      return;
    }
  }    
  *(a + i) = x;
  return;      


  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}




template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i,  less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return; 

  
  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

} 

#endif // __UTILITY_FUNCTIONS__


